import socket
import json
import threading
import time
from queue import Queue
import select
from movj import Robotmove # 从movj中引入类robotmove

class TCPServer:
    def __init__(self, host='127.0.0.0', port=9999):
        self.host = host
        self.port = port
        self.server_socket = None
        self.clients = {}  # {client_address: socket}
        self.running = False
        self.lock = threading.Lock()
        self.result_queue = Queue()
        self.robot_arm = Robotmove()  # 创建 RobotArm 实例
        self.robot_arm.set_crash_level(1)

    def start(self):
        self.running = True
        self.server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
        self.server_socket.bind((self.host, self.port))
        self.server_socket.listen(5)
        
        # Start the main server thread
        server_thread = threading.Thread(target=self._accept_connections)
        server_thread.daemon = True
        server_thread.start()
        
        # # Start the client monitor thread
        # monitor_thread = threading.Thread(target=self._monitor_clients)
        # monitor_thread.daemon = True
        # monitor_thread.start()
        
    def stop(self):
        self.running = False
        if self.server_socket:
            self.server_socket.close()
        with self.lock:
            for client in self.clients.values():
                try:
                    client.close()
                except:
                    pass
            self.clients.clear()
            
    def _accept_connections(self):
        while self.running:
            try:
                readable, _, _ = select.select([self.server_socket], [], [], 1.0)
                if self.server_socket in readable:
                    client_socket, client_address = self.server_socket.accept()
                    client_socket.settimeout(60)  # Set timeout for client operations
                    with self.lock:
                        self.clients[client_address] = client_socket
                    
                    # Start a new thread to handle this client
                    client_thread = threading.Thread(
                        target=self._handle_client,
                        args=(client_socket, client_address)
                    )
                    client_thread.daemon = True
                    client_thread.start()
            except Exception as e:
                print(f"Error in accept_connections: {e}")
                time.sleep(1)

    def _handle_client(self, client_socket, client_address):
        while self.running:
            try:
                data = client_socket.recv(1024)
                if not data:
                    break
                
                command = data.decode('utf-8').strip()
                
                # 处理 "TEST" 命令
                if command == "close":
                    self.stop()
                    response = "服务器关闭..." 
                elif command.startswith('A'):
                    id=command[-1]
                    # print(id,"!!!!!!!!!!!")
                    if self.robot_arm.is_connected:
                        self.robot_arm.move_joints(id)
                        response = f"command A{id} received"
                    else:
                        response = "未与机械臂连接"
                # elif command.startswith('A2'):
                #     if self.robot_arm.is_connected:
                #         self.robot_arm.move_joints(self.robot_arm.J2)
                #         response = "command A2 received"
                #     else:
                #         response = "未与机械臂连接"
                else:
                    response = "未知命令"  # 处理其他命令的默认响应
                
                # 发送响应回客户端
                client_socket.sendall(response.encode('utf-8'))
            
            except socket.timeout:
                print(f"客户端 {client_address} 超时，正在继续...")
                continue
            except Exception as e:
                print(f"处理客户端 {client_address} 时发生错误: {e}")
                

        # 清理客户端连接
        with self.lock:
            if client_address in self.clients:
                del self.clients[client_address]
        # try:
        #     client_socket.close()
        # except:
        #     pass           
      
    def _monitor_clients(self):
        while self.running:
            with self.lock:
                # Create a copy of clients to avoid modification during iteration
                clients = list(self.clients.items())
                
            for client_address, client_socket in clients:
                try:
                    # Try to send a heartbeat
                    client_socket.send(b'ping')
                except:
                    # If failed, remove the client and close the socket
                    with self.lock:
                        if client_address in self.clients:
                            del self.clients[client_address]
                    try:
                        client_socket.close()
                    except:
                        pass
                        
            time.sleep(5)  # Check every 5 seconds
            
    def update_result(self, result):
        """Update the latest test result"""
        # Clear the queue and put the new result
        while not self.result_queue.empty():
            try:
                self.result_queue.get_nowait()
            except Queue.Empty:
                break
        self.result_queue.put(result)

# Create a global TCP server instance
tcp_server = None

if __name__ == "__main__":
    # 创建 TCP 服务器实例
    tcp_server = TCPServer(host='127.0.0.1', port=9999)
    
    # 启动服务器
    tcp_server.start()
    
    print("TCP 服务器已启动，等待客户端连接...")
    
    try:
        while True:
            time.sleep(1)  # 主线程保持运行
    except KeyboardInterrupt:
        print("正在关闭服务器...")
        tcp_server.stop()
        print("服务器已关闭。")
    except Exception as e:
        print(f"发生错误: {e}")
    finally:
        # 确保在关闭服务器时清理资源
        tcp_server.stop()
        print("服务器已关闭。")